#include "region_model.h"

bool RegionModel::GetClosestView(
  const Pose& body2camera_pose,
  const View** closest_view) const 
{
  if (body2camera_pose.translation().norm() == 0.0f) {
    *closest_view = &views_[0];
    return true;
  }

  Eigen::Vector3f orientation{
      body2camera_pose.rotation().inverse() *
      body2camera_pose.translation().matrix().normalized() };

  float closest_dot = -1.0f;
  for (auto& view : views_) {
    float dot = orientation.dot(view.orientation);
    if (dot > closest_dot) {
      *closest_view = &view;
      closest_dot = dot;
    }
  }
  return true;
}